﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using NIP.ControllerInterface;
using NIP.DynamicControl.Constants;

namespace NIP.DynamicControl.Environ
{
    public class PathPoint
    {
        public double[] pos;
        public double[] vel;
        public double[] acc;
        private double[] joint;
        private double offset;
        public double WristPitch;
        
        public PathPoint()
        {
            //Debug.WriteLine("Default Path Point created");
            pos = new double[USBC.TotalAxes];
            vel = new double[USBC.TotalAxes];
            acc = new double[USBC.TotalAxes];
            joint = new double[USBC.TotalAxes];
        }

        public PathPoint(Position j, double[] v, double[] a)
        {
            pos = new double[USBC.TotalAxes];
            vel = new double[USBC.TotalAxes];
            acc = new double[USBC.TotalAxes];
            joint = new double[USBC.TotalAxes];
            offset = j.Offsetd;
            for (int i = 0; i < 5; i++)
                joint[i] = j.Jointd[i];
            joint[0] -= gridconst.theta1fudge;
            for (int i = 0; i < 3; i++)
            {
                vel[i] = v[i];
                acc[i] = a[i];
            }
            setWristPitch();
            pos[0] = gridconst.L0 + (gridconst.L2 * Math.Cos(gridconst.toRadians(joint[1]))) + (gridconst.L3 * Math.Cos(gridconst.toRadians(joint[1] + joint[2]))) + (gridconst.D5grip * Math.Cos(gridconst.toRadians(joint[1] + joint[2] + joint[3])));
            pos[1] = pos[0];
            pos[0] *= Math.Cos(gridconst.toRadians(joint[0]));
            pos[1] *= Math.Sin(gridconst.toRadians(joint[0]));
            pos[0] += gridconst.L4 * Math.Sin(gridconst.toRadians(joint[0]));
            pos[1] -= gridconst.L4 * Math.Cos(gridconst.toRadians(joint[0]));
            pos[2] = gridconst.L1 - (gridconst.L2 * Math.Sin(gridconst.toRadians(joint[1]))) - (gridconst.L3 * Math.Sin(gridconst.toRadians(joint[1] + joint[2]))) - (gridconst.D5grip * Math.Sin(gridconst.toRadians(joint[1] + joint[2] + joint[3])));
        }

        public PathPoint(Position j)
        {
            pos = new double[USBC.TotalAxes];
            vel = new double[USBC.TotalAxes];
            acc = new double[USBC.TotalAxes];
            joint = new double[USBC.TotalAxes];
            offset = j.Offsetd;
            for (int i = 0; i < 5; i++)
                joint[i] = j.Jointd[i];
            joint[0] -= gridconst.theta1fudge;
            setWristPitch();
            pos[0] = gridconst.L0 + (gridconst.L2 * Math.Cos(gridconst.toRadians(joint[1]))) + (gridconst.L3 * Math.Cos(gridconst.toRadians(joint[1] + joint[2]))) + (gridconst.D5grip * Math.Cos(gridconst.toRadians(joint[1] + joint[2] + joint[3])));
            pos[1] = pos[0];
            pos[0] *= Math.Cos(gridconst.toRadians(joint[0]));
            pos[1] *= Math.Sin(gridconst.toRadians(joint[0]));
            pos[0] += gridconst.L4 * Math.Sin(gridconst.toRadians(joint[0]));
            pos[1] -= gridconst.L4 * Math.Cos(gridconst.toRadians(joint[0]));
            pos[2] = gridconst.L1 - (gridconst.L2 * Math.Sin(gridconst.toRadians(joint[1]))) - (gridconst.L3 * Math.Sin(gridconst.toRadians(joint[1] + joint[2]))) - (gridconst.D5grip * Math.Sin(gridconst.toRadians(joint[1] + joint[2] + joint[3])));

        }

        public PathPoint(PathPoint p)
        {
            pos = new double[USBC.TotalAxes];
            vel = new double[USBC.TotalAxes];
            acc = new double[USBC.TotalAxes];
            joint = new double[USBC.TotalAxes];
            offset = p.offset;
            for (int i = 0; i < USBC.TotalAxes; i++)
            {
                joint[i] = p.joint[i];
                pos[i] = p.pos[i];
                vel[i] = p.vel[i];
                acc[i] = p.acc[i];
            }
            WristPitch = p.WristPitch;
        }
        
        private void setWristPitch()
        {
            WristPitch = -(joint[1] + joint[2] + joint[3]);
        }

        public double getOffset()
        {
            return offset;
        }

        public void setOffset(double o)
        {
            offset = 0;
        }

        public void setJoint(int j, double a)
        {
            joint[j] = a;
            setWristPitch();
        }

        public double getJoint(int j)
        {
            return joint[j];
        }

        public double getPitch()
        {
            return WristPitch;
        }

        public void setRoll(double roll)
        {
            if (roll < gridconst.theta5min)
                joint[4] = (roll + 360.0);
            else if (roll > gridconst.theta5max)
                joint[4] = (roll - 360.0);
            else
                joint[4] = roll;
        }

        public double getRoll()
        {
            return joint[4];
        }
    }



}
